#pragma once

#include <iostream>
#include <vector>
#include "Polynomial.h"
//#include <lapacke.h>        //lapack使用教程详见README,事实上并没有用到
#include <eigen3/Eigen/Dense>

using namespace std;
using namespace Eigen;

//1.1 5种 cubic spline  piecewise-polynomial Form
class spline
{
    private:
    double EPS = 0.00000001;
    int n_node;         //节点个数
    int deg = 3;        //多项式的最高次数、阶,默认为3
    int k = 2;          //smoothness class， 默认为2
    std::vector<double> node = {};          //x
    std::vector<double> value = {};         //f(x)

    private:
    double dfa;
    double dfb;

    private:
    std::vector<double> miu = {0};        //i = 2,...,N-1 , 前面占位1个0以统一下标, 即占记miu1=0
    std::vector<double> lambda = {0};     //i = 2,...,N-1   ,长为N-1
    std::vector<double> K = {};            //i = 1,...,N-1
    void init();
    std::vector<double> solve(std::vector<double> b);   //解三对角矩阵
    //std::vector<double> solve(std::vector<double> a,std::vector<double> b,std::vector<double> c,std::vector<double> d);   //解三对角矩阵

    public:
    spline(){};
    spline(std::vector<double> _node, std::vector<double> _value);
    std::vector<Polynomial> complete(double, double);
    std::vector<Polynomial> specified(double,double);
    std::vector<Polynomial> natual();
    std::vector<Polynomial> not_a_knot();
    std::vector<Polynomial> periodic();

    public:
    std::vector<Polynomial> polys;
    double fit(double x);
    void show_polys();
    
};

//1.2 func for spline
//create func
spline::spline(std::vector<double> _node, std::vector<double> _value)
{
    node = _node;
    value = _value;
    n_node = _node.size();
    deg = 3;
    k = 2;
    if (n_node != _value.size())
    {
        std::cout << "error! 点与函数值不等长" << std::endl;
    }
}

//calculate miu,lambda,K
void spline::init()
{
    if (n_node < 2) return;
    double t1,t2,t3;
    K.push_back( (value[1] - value[0]) / (node[1] - node[0]) );
    for (int i = 1; i < n_node-1; i++)
    {
        miu.push_back((node[i]-node[i-1])/(node[i+1]-node[i-1]));
        lambda.push_back((node[i+1]-node[i])/(node[i+1]-node[i-1]));
        K.push_back((value[i+1]-value[i])/(node[i+1]-node[i]));
    }
}

std::vector<Polynomial> spline::complete(double dfa, double dfb)
{
    init();
    vector<Polynomial> fit;
    
    std::vector<double> b{};
    for(int i = 1; i < n_node-1; i++)
    {
        b.push_back(3*miu[i]*K[i]+3*lambda[i]*K[i-1]);
    }

    b[0] -= lambda[1]*dfa;  //b[0]
    b[n_node-3] -= miu[n_node-2]*dfb ;  //b[N-2]

    std::vector<double> m = solve(b);   //m为m1-mN所有
    m[0] = dfa;
    m[n_node-1] = dfb;

    for(int i = 0; i < n_node-1; i++)
    {
        Polynomial temp(-1*node[i],1);
        Polynomial c0(value[i]);
        double c1 = m[i];
        double c2 = (3*K[i]-2*m[i]-m[i+1])/(node[i+1]-node[i]);
        double c3 = (m[i]+m[i+1]-2*K[i])/(node[i+1]-node[i])/(node[i+1]-node[i]);
        Polynomial p = c0 + temp * c1 + temp * temp * c2 + temp * temp * temp * c3;
        fit.push_back(p);
    }
    polys = fit;

    return fit;
}

std::vector<double> spline::solve(std::vector<double> b)
{
    std::vector<double> m = {};
    m.resize(n_node);
    miu[1] /= 2;
    b[0] /= 2;
    int i = 2;
    for(i = 2;i < n_node-2;i++)
    {
        miu[i] /= 2-miu[i-1]*lambda[i];
        b[i-1] = (b[i-1]-b[i-2]*lambda[i]) / (2 - miu[i-1] * lambda[i]);
    }
    b[i-1] = (b[i-1]-b[i-2]*lambda[i]) / (2 - miu[i-1] * lambda[i]);


    m[n_node-2] = (b[n_node-3]);
    for(int i = n_node-3; i >= 1; i--)
    {
        m[i] = (b[i-1]-miu[i]*m[i+1]);
        //cout << "m[i]=" << m[i] << "\t" << "b[i]=" << b[i] << "\t" << miu[i] << "\t" << m[i+1] << endl ;
    }
    return m;
}

double spline::fit(double x)
{
    if (n_node == 0) cout << "未输入点" << endl;
    for (int i = 0; i < n_node-1; i++)
    {
        if (node[i] <= x && x <= node[i+1])
        {
            if (polys.size() <= i) 
            {
                cout << "未生成样条" << endl;
                break;
            }
            return polys[i](x);
        }
    }
    cout << "超出定义域" << endl;
    return 0;
}

void spline::show_polys()
{
    for (int i = 0; i < polys.size(); i++)
    {
        polys[i].show_poly();
    }
}


std::vector<Polynomial> spline::specified(double ddfa,double ddfb)
{
    init();
    int N = n_node;
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N, N);
    Eigen::VectorXd rhs = Eigen::VectorXd::Zero(N);

    A(0,0) = 1;
    rhs[0] = ddfa;
    A(N-1,N-1) = 1;
    rhs[N-1] = ddfb;

    for (size_t i = 1; i < n_node - 1; i++)
    {
        A(i, i) = 2;
        A(i, i - 1) = miu[i];
        A(i, i + 1) = 1.0 - A(i, i - 1);
        rhs[i] = ( 6*(K[i] - K[i-1])/(node[i+1]-node[i-1]));
    }

    Eigen::VectorXd M = A.lu().solve(rhs); 
    
    vector<Polynomial> fit;

    for (size_t i = 0; i < N - 1; i++)
    {
        Polynomial c_0(value[i]);                                         
        Polynomial p(-1*node[i],1);   
        double c_1 = K[i] - (2 * M(i) + M(i + 1)) * (node[i + 1] - node[i]) / 6.0;
        double c_3 = (M(i + 1) - M(i)) / (node[i + 1] - node[i]) / 6.0;
        Polynomial res = c_0 + p * c_1 +  p * p * (M(i) / 2.0) + p * p * p * c_3; 
        fit.push_back(res);
    }
    polys = fit;

    return fit;
}


std::vector<Polynomial> spline::natual()
{
    return specified(0,0);
}

std::vector<Polynomial> spline::not_a_knot()
{
    if (n_node < 4) cout << "error!n 要大于3" << endl;
    init();
    int N = n_node;
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N, N);
    Eigen::VectorXd rhs = Eigen::VectorXd::Zero(N);

    A(0,0) = 1;
    double temp = (node[0]-node[1])/(node[2]-node[1]);
    A(0,1) = temp-1;
    A(0,2) = -1 * temp;
    temp = (node[N-1]-node[N-2])/(node[N-2]-node[N-3]);
    A(N-1,N-3) = temp;
    A(N-1,N-2) = (temp+1)*(-1);
    A(N-1,N-1) = 1;

    for (size_t i = 1; i < n_node - 1; i++)
    {
        A(i, i) = 2;
        A(i, i - 1) = miu[i];
        A(i, i + 1) = 1.0 - A(i, i - 1);
        rhs[i] = ( 6*(K[i] - K[i-1])/(node[i+1]-node[i-1]));
    }

    Eigen::VectorXd M = A.lu().solve(rhs); 
    
    vector<Polynomial> fit;

    for (size_t i = 0; i < N - 1; i++)
    {
        Polynomial c_0(value[i]);                                         
        Polynomial p(-1*node[i],1);   
        double c_1 = K[i] - (2 * M(i) + M(i + 1)) * (node[i + 1] - node[i]) / 6.0;
        double c_3 = (M(i + 1) - M(i)) / (node[i + 1] - node[i]) / 6.0;
        Polynomial res = c_0 + p * c_1 +  p * p * (M(i) / 2.0) + p * p * p * c_3; 
        fit.push_back(res);
    }
    polys = fit;

    return fit;
}
std::vector<Polynomial> spline::periodic()
{
    if (fabs(value[0] - value[n_node-1]) > EPS)
    {
        cout << "error! 不满足周期性条件" << endl;
    }
    init();
    int N = n_node;
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(N, N);
    Eigen::VectorXd rhs = Eigen::VectorXd::Zero(N);

    A(0,0) = 2 * (node[1] - node[0]);
    A(0,1) = (node[1] - node[0]);
    A(0,N-2) = (node[N-1] - node[N-2]);
    A(0,N-1) = 2 * (node[N-1] - node[N-2]);
    rhs[0] = 6 * (K[0] - K[N-2]);

    A(N-1,0) = 1;
    A(N-1,N-1) = -1;

    for (size_t i = 1; i < n_node - 1; i++)
    {
        A(i, i) = 2;
        A(i, i - 1) = miu[i];
        A(i, i + 1) = 1.0 - A(i, i - 1);
        rhs[i] = ( 6*(K[i] - K[i-1])/(node[i+1]-node[i-1]));
    }

    Eigen::VectorXd M = A.lu().solve(rhs); 
    
    vector<Polynomial> fit;

    for (size_t i = 0; i < N - 1; i++)
    {
        Polynomial c_0(value[i]);                                         
        Polynomial p(-1*node[i],1);   
        double c_1 = K[i] - (2 * M(i) + M(i + 1)) * (node[i + 1] - node[i]) / 6.0;
        double c_3 = (M(i + 1) - M(i)) / (node[i + 1] - node[i]) / 6.0;
        Polynomial res = c_0 + p * c_1 +  p * p * (M(i) / 2.0) + p * p * p * c_3; 
        fit.push_back(res);
    }
    polys = fit;
    return fit;
}

double distance(double x1, double y1, double x2, double y2)
{
    return sqrt(pow(x1-x2,2) + pow(y1-y2,2));
}

class curve_spline
{
    private:
    vector<double> t;
    int N;
    spline B_x,B_y;
    double length;

    public:
    vector<double> x,y;

    public:
    curve_spline(vector<double> _x,vector<double> _y)
    {
        N = _x.size();
        if (N != _y.size()) cout << "x and y not match" << endl;
        x = _x;
        y = _y;
    }
    void fit()
    {
        t.push_back(0);
        for (int i = 1; i < N; i++)
        {
            t.push_back(t[i-1] + distance(x[i-1],y[i-1],x[i],y[i]));
        }
        /*
        for (int i = 0; i < N; i++)
        {
            cout << "i = " << i << "\t" << t[i] << "\t" << x[i] << "\t" << y[i] << endl;
        }*/
        
        spline Bx(t,x),By(t,y);
        B_x = Bx;
        B_y = By;
        B_x.periodic();
        B_y.periodic();
        length = t[N-1];
    }
    double len()
    {
        return length;
    }
    double X(double t)
    {
        return B_x.fit(t);
    }
    double Y(double t)
    {
        return B_y.fit(t);
    }

};